#include "troyglcontroller.h"

TroyGLController::TroyGLController(QObject* parent) : RobotGLController(parent)
{
    _recentCommand = NULL;
    _dragCommand = NULL;
    _prevX = 0;
    _prevY = 0;
    _prevZ = 0;
    _mouseDrag = false;
}

void TroyGLController::initComponents(){
    //The model should be stored now, so we can completely
    //initialize the GUI components.

    //We need to create an Component struct for each object
    //of the robot that can be selected, etc.

    //Body
//    GUIComponent* curComponent = MALLOC_COMPONENT;
//    curComponent->numOfNames = 1;
//    curComponent->names[0] = T_BODY;
//    curComponent->names[1] = NULL;
//    curComponent->names[2] = NULL;
//    curComponent->names[3] = NULL;
//    curComponent->names[4] = NULL;
//    curComponent->selectable = false;
//    curComponent->selected = false;
//    curComponent->selectionColor[0] = 0;
//    curComponent->selectionColor[1] = 1.0f;
//    curComponent->selectionColor[2] = 1.0;
//    _components->append(curComponent);

//    //Neck
//    curComponent = MALLOC_COMPONENT;
//    curComponent->numOfNames = 1;
//    curComponent->names[0] = T_NECK;
//    curComponent->names[1] = NULL;
//    curComponent->names[2] = NULL;
//    curComponent->names[3] = NULL;
//    curComponent->names[4] = NULL;
//    curComponent->selectable = true;
//    curComponent->selected = false;
//    curComponent->selectionColor[0] = 1.0f;
//    curComponent->selectionColor[1] = 0;
//    curComponent->selectionColor[2] = 1.0f;
//    _components->append(curComponent);
//    //Head
//    curComponent = MALLOC_COMPONENT;
//    curComponent->numOfNames = 1;
//    curComponent->names[0] = T_HEAD;
//    curComponent->names[1] = NULL;
//    curComponent->names[2] = NULL;
//    curComponent->names[3] = NULL;
//    curComponent->names[4] = NULL;
//    curComponent->selectable = true;
//    curComponent->selected = false;
//    curComponent->selectionColor[0] = 0;
//    curComponent->selectionColor[1] = 1.0f;
//    curComponent->selectionColor[2] = 1.0f;
//    _components->append(curComponent);
//    //Left side
//    //Shoulder
//    curComponent = MALLOC_COMPONENT;
//    curComponent->numOfNames = 2;
//    curComponent->names[0] = T_LEFT;
//    curComponent->names[1] = T_SHOULDER;
//    curComponent->names[2] = NULL;
//    curComponent->names[3] = NULL;
//    curComponent->names[4] = NULL;
//    curComponent->selectable = true;
//    curComponent->selected = false;
//    curComponent->selectionColor[0] = 1.0f;
//    curComponent->selectionColor[1] = 0;
//    curComponent->selectionColor[2] = 1.0f;
//    //There has to be some "magic number" somewhere!
//    curComponent->component = _model->findComponentWithJoint(0);
//    _components->append(curComponent);
//    //Arm
//    curComponent = MALLOC_COMPONENT;
//    curComponent->numOfNames = 2;
//    curComponent->names[0] = T_LEFT;
//    curComponent->names[1] = T_ARM;
//    curComponent->names[2] = NULL;
//    curComponent->names[3] = NULL;
//    curComponent->names[4] = NULL;
//    curComponent->selectable = true;
//    curComponent->selected = false;
//    curComponent->selectionColor[0] = 0;
//    curComponent->selectionColor[1] = 1.0f;
//    curComponent->selectionColor[2] = 1.0f;
//    curComponent->component = _model->findComponentWithJoint(0);
//    _components->append(curComponent);
//    //Elbow
//    curComponent = MALLOC_COMPONENT;
//    curComponent->numOfNames = 2;
//    curComponent->names[0] = T_LEFT;
//    curComponent->names[1] = T_ELBOW;
//    curComponent->names[2] = NULL;
//    curComponent->names[3] = NULL;
//    curComponent->names[4] = NULL;
//    curComponent->selectable = true;
//    curComponent->selected = false;
//    curComponent->selectionColor[0] = 1.0f;
//    curComponent->selectionColor[1] = 0;
//    curComponent->selectionColor[2] = 1.0f;
//    curComponent->component = _model->findComponentWithJoint(9);
//    _components->append(curComponent);
//    //Forearm
//    curComponent = MALLOC_COMPONENT;
//    curComponent->numOfNames = 2;
//    curComponent->names[0] = T_LEFT;
//    curComponent->names[1] = T_FOREARM;
//    curComponent->names[2] = NULL;
//    curComponent->names[3] = NULL;
//    curComponent->names[4] = NULL;
//    curComponent->selectable = true;
//    curComponent->selected = false;
//    curComponent->selectionColor[0] = 0;
//    curComponent->selectionColor[1] = 1.0f;
//    curComponent->selectionColor[2] = 1.0f;
//    curComponent->component = _model->findComponentWithJoint(9);
//    _components->append(curComponent);
//    //Right side
//    //Shoulder
//    curComponent = MALLOC_COMPONENT;
//    curComponent->numOfNames = 2;
//    curComponent->names[0] = T_RIGHT;
//    curComponent->names[1] = T_SHOULDER;
//    curComponent->names[2] = NULL;
//    curComponent->names[3] = NULL;
//    curComponent->names[4] = NULL;
//    curComponent->selectable = true;
//    curComponent->selected = false;
//    curComponent->selectionColor[0] = 1.0f;
//    curComponent->selectionColor[1] = 0;
//    curComponent->selectionColor[2] = 1.0f;
//    curComponent->component = _model->findComponentWithJoint(16);
//    _components->append(curComponent);
//    //Arm
//    curComponent = MALLOC_COMPONENT;
//    curComponent->numOfNames = 2;
//    curComponent->names[0] = T_RIGHT;
//    curComponent->names[1] = T_ARM;
//    curComponent->names[2] = NULL;
//    curComponent->names[3] = NULL;
//    curComponent->names[4] = NULL;
//    curComponent->selectable = true;
//    curComponent->selected = false;
//    curComponent->selectionColor[0] = 0;
//    curComponent->selectionColor[1] = 1.0f;
//    curComponent->selectionColor[2] = 1.0f;
//    curComponent->component = _model->findComponentWithJoint(16);
//    _components->append(curComponent);
//    //Elbow
//    curComponent = MALLOC_COMPONENT;
//    curComponent->numOfNames = 2;
//    curComponent->names[0] = T_RIGHT;
//    curComponent->names[1] = T_ELBOW;
//    curComponent->names[2] = NULL;
//    curComponent->names[3] = NULL;
//    curComponent->names[4] = NULL;
//    curComponent->selectable = true;
//    curComponent->selected = false;
//    curComponent->selectionColor[0] = 1.0f;
//    curComponent->selectionColor[1] = 0;
//    curComponent->selectionColor[2] = 1.0f;
//    curComponent->component = _model->findComponentWithJoint(25);
//    _components->append(curComponent);
//    //Forearm
//    curComponent = MALLOC_COMPONENT;
//    curComponent->numOfNames = 2;
//    curComponent->names[0] = T_RIGHT;
//    curComponent->names[1] = T_FOREARM;
//    curComponent->names[2] = NULL;
//    curComponent->names[3] = NULL;
//    curComponent->names[4] = NULL;
//    curComponent->selectable = true;
//    curComponent->selected = false;
//    curComponent->selectionColor[0] = 0;
//    curComponent->selectionColor[1] = 1.0f;
//    curComponent->selectionColor[2] = 1.0f;
//    curComponent->component = _model->findComponentWithJoint(25);
//    _components->append(curComponent);

    //clearSelection();

}


void TroyGLController::componentPicked(int numOfNames, unsigned int* names){
    //For our application, picked = selected
    GUIComponent* selected = selectComponent(numOfNames, names);
    if(selected != NULL){
        //Tell the view to display the selection
        _view->selectComponent(selected);
    }
}


void TroyGLController::mousePress(QMouseEvent *event){
    //Begin tracking for dragging
    _mouseDrag = true;
    _view->transformTo3D((float)event->x(), (float)event->y(),
                         &_prevX, &_prevY, &_prevZ);
}

void TroyGLController::mouseRelease(QMouseEvent* event){
    //Tell the view to clear the selection
    _view->deselectComponent(_selected);
    clearSelection();

    //End the dragging
    _mouseDrag = false;
    //Store the drag in the history
    if(_dragCommand != NULL){
        History::Instance()->push(_dragCommand);
        _dragCommand = NULL;
    }
}

void TroyGLController::mouseMove(QMouseEvent *event){
    double posX, posY, posZ;
    _view->transformTo3D((float)event->x(), (float)event->y(),
                         &posX, &posY, &posZ);


    emit mouseMoveEvent(posX, posY, posZ);

    if(_mouseDrag){
        mouseDrag((posX - _prevX),(posY - _prevY), (posZ - _prevZ));

        _prevX = posX;
        _prevY = posY;
        _prevZ = posZ;
    }
}


void TroyGLController::mouseDrag(double deltaX, double deltaY, double deltaZ){
//    float trueDeltaY;
//    RobotCommandGroup* command;

//    emit mouseDragEvent(deltaX, deltaY, deltaZ);

//    if(_selected != NULL){

//        //Figure out what joint to manipulate
//        if(_selected->numOfNames == 1){
//            //Head or neck
//        }
//        else if(_selected->numOfNames == 2){
//            //Arms
//            if(_selected->names[0] == T_LEFT){
//                if(_selected->names[1] == T_ELBOW || _selected->names[1] == T_FOREARM){
//                    //Create a robot command group
//                    command = new RobotCommandGroup(_model);
//                    command->append(new MoveRobotJointCommand(_model, 9, deltaX * 100));
//                    command->append(new MoveRobotJointCommand(_model, 0, -deltaY * 100));

//                    //Append the command (group) to a command group
//                    //that is being created as we drag.  The drag command group
//                    //will be placed in the history to undo/redo the whole drag
//                    if(_dragCommand == NULL)
//                        _dragCommand = new RobotCommandGroup(_model);
//                    _dragCommand->append(command);

//                    command->execute();
//                }
//                else if(_selected->names[1] == T_SHOULDER || _selected->names[1] == T_ARM){

//                }
//            }
//            else if(_selected->names[0] == T_RIGHT){
//                if(_selected->names[1] == T_ELBOW || _selected->names[1] == T_FOREARM){
//                //Create a robot command group
//                command = new RobotCommandGroup(_model);
//                command->append(new MoveRobotJointCommand(_model, 25, -deltaX));

////                    //Transform the movement into angles
//                trueDeltaY = atan2((float)deltaY / 10.0f, 10.0f);
////                    //convert from radians to degrees
//                trueDeltaY = trueDeltaY * (180 / M_PI);
////                    qDebug() << "True delta Y: " << trueDeltaY;

//                command->append(new MoveRobotJointCommand(_model, 16, trueDeltaY));

//                //Append the command (group) to a command group
//                //that is being created as we drag.  The drag command group
//                //will be placed in the history to undo/redo the whole drag
//                if(_dragCommand == NULL)
//                    _dragCommand = new RobotCommandGroup(_model);
//                _dragCommand->append(command);

//                command->execute();
//            }
//            }
//        }
//    }
}
